close all; clear; clc;

Alpha = [0, pi/2, 0, pi/2, -pi/2, pi/2];
A = [0, 0.18, 0.58, 0.16, 0, 0];
D = [0.43, 0, 0, 0.6395, 0, 0.115];
Theta = [0, pi/2, 0, 0, 0, 0];
Sigma = [1, 1, 1, 1, 1, 1];

DH_para.Alpha = Alpha;
DH_para.A = A;
DH_para.D = D;
DH_para.Theta = Theta;
DH_para.Sigma = Sigma;

T0 = troty(pi/3) * trotz(pi);
pxs = 0.5; pxn = -0.5;
pys = 0.5; pyn = -0.5;
pzs = 1.2; pzn = 1.2;
Ts = T0 * transl(pxs, pys, pzs);
Tn = T0 * transl(pxn, pyn, pzn);

tn = 10;
dt = 0.1;
t = 0:dt:tn;

q0 = [0, 0, 0, 0, 0, 0];

num = length(t);

q1_1 = zeros(num, 1);
q2_1 = zeros(num, 1);
q3_1 = zeros(num, 1);
q4_1 = zeros(num, 1);
q5_1 = zeros(num, 1);
q6_1 = zeros(num, 1);

q1_2 = zeros(num, 1);
q2_2 = zeros(num, 1);
q3_2 = zeros(num, 1);
q4_2 = zeros(num, 1);
q5_2 = zeros(num, 1);
q6_2 = zeros(num, 1);

for i = 1:length(t)
    px = pxs + (pxn - pxs) * t(i) / tn;
    py = pys + (pyn - pys) * t(i) / tn;
    pz = pzs + (pzn - pzs) * t(i) / tn;
    T = transl(px, py, pz) * T0;
    
    if i == 1
        q0 = q0;
    else
        q0 = [q1_1(i - 1), q2_1(i - 1),q3_1(i - 1),q4_1(i - 1),q5_1(i - 1),q6_1(i - 1)];
    end
    
    qn = ikine(T, q0, DH_para, 0, 0, 0);
    q1_1(i) = qn(1);
    q2_1(i) = qn(2);
    q3_1(i) = qn(3);
    q4_1(i) = qn(4);
    q5_1(i) = qn(5);
    q6_1(i) = qn(6);
    
    if i == 1
        q0 = q0;
    else
        q0 = [q1_2(i - 1), q2_2(i - 1),q3_2(i - 1),q4_2(i - 1),q5_2(i - 1),q6_2(i - 1)];
    end
    qn = ikine(T, q0, DH_para, 1, 0, 0);
    
    q1_2(i) = qn(1);
    q2_2(i) = qn(2);
    q3_2(i) = qn(3);
    q4_2(i) = qn(4);
    q5_2(i) = qn(5);
    q6_2(i) = qn(6);
end

qs = [q1_1, q2_1, q3_1, q4_1, q5_1, q6_1];
d_wcp_min_index = overhead_singularity_detect(qs, DH_para);

q1 = q1_1; q1(d_wcp_min_index:end) = q1_2(d_wcp_min_index:end);
q2 = q2_1; q2(d_wcp_min_index:end) = q2_2(d_wcp_min_index:end);
q3 = q3_1; q3(d_wcp_min_index:end) = q3_2(d_wcp_min_index:end);
q4 = q4_1; q4(d_wcp_min_index:end) = q4_2(d_wcp_min_index:end);
q5 = q5_1; q5(d_wcp_min_index:end) = q5_2(d_wcp_min_index:end);
q6 = q6_1; q6(d_wcp_min_index:end) = q6_2(d_wcp_min_index:end);

interpo_num = 30;
q1_new = interpolate(q1, d_wcp_min_index, interpo_num);

for i = 1:length(t)
    px_desired(i) = pxs + (pxn - pxs) * t(i) / tn;
    py_desired(i) = pys + (pyn - pys) * t(i) / tn;
    pz_desired(i) = pzs + (pzn - pzs) * t(i) / tn;
    T = transl(px_desired(i), py_desired(i), pz_desired(i)) * T0;
    
    if i < d_wcp_min_index-interpo_num || i > d_wcp_min_index+interpo_num
        qn = [q1(i), q2(i), q3(i), q4(i), q5(i), q6(i)];
    else 
        qn = ikine2(T, q0, DH_para, q1_new(i), 0, 0, 0);
    end

    q0 = qn;
    q(i, :) = qn;
    
    T_actual = fkine(qn, DH_para);
    px_actual(i) = T_actual(1, 4);
    py_actual(i) = T_actual(2, 4);
    pz_actual(i) = T_actual(3, 4);
end

dq = diff(q) / dt;
ddq = diff(dq) / dt;

figure(1)
for i = 1:6
subplot(3, 2, i);
plot(t, q(:, i));
xlabel("t (s)");
ylabel("q" + i + " (rad)");
end

figure(2)
for i = 1:6
subplot(3, 2, i)
plot(t(1:end-1), dq(:, i));
xlabel("t (s)");
ylabel("dq" + i + " (rad/s)");
end

figure(3)
for i = 1:6
subplot(3, 2, i)
plot(t(1:end-2), ddq(:, i));
xlabel("t (s)");
ylabel("ddq" + i + " (rad/s^2)");
end

figure(4);
subplot(3, 1, 1);
plot(t, px_desired);
hold on;
plot(t, px_actual, '--');
xlabel("t (s)");
ylabel("px" + " (m)");
legend("desired", "actual");

subplot(3, 1, 2);
plot(t, py_desired);
hold on;
plot(t, py_actual, '--');
xlabel("t (s)");
ylabel("py" + " (m)");
legend("desired", "actual");

subplot(3, 1, 3);
plot(t, pz_desired);
hold on;
plot(t, pz_actual, '--');
xlabel("t (s)");
ylabel("pz" + " (m)");
legend("desired", "actual");
ylim([1, 1.5]);


q0_timeseries = timeseries([q1_1, q2_1, q3_1, q4_1, q5_1, q6_1], t);
q1_timeseries = timeseries(q, t);
